/*----------------------------------------------------------------------------*/
/* Team 3647
 /* Auton.java
 /* Auton Routine
 /*----------------------------------------------------------------------------*/
package Robot;

public class Auton {
    
    public static final double RPM = 2650;
    private static long startTime = 0;
    
    public static void resetAuton() {
        startTime = System.currentTimeMillis();
        Tower.towerLift.set(0);
    }
    static long startLift = 2000;
    static long oneBallShotStop = startLift + 500;
    static long secondBallStartShot = oneBallShotStop + 1000;
    
    public static void AutonProcess() {
        
        Shooter.BangBang(RPM);
        Tower.StopOut();
        
        long elapsed = System.currentTimeMillis() - startTime;
        
        if (elapsed > startLift && elapsed < oneBallShotStop) {
            Tower.towerLift.set(1.0);
        } else if (elapsed > oneBallShotStop && elapsed < secondBallStartShot) {
            Tower.towerLift.set(0);
        } else if (elapsed > secondBallStartShot && elapsed < (secondBallStartShot + 500)) {
            Tower.towerLift.set(1.0);
        } else if (elapsed > (secondBallStartShot + 500) && elapsed < (secondBallStartShot + 500 + 1000)) {
            Tower.towerLift.set(0);
        } else if (elapsed > (secondBallStartShot + 500 + 1000)) {
            Tower.towerLift.set(1);
        }
    }
}
